#include <Servo.h> 

#define PORT_BRAS_DROIT 7
#define PORT_BRAS_GAUCHE 5
#define PORT_TETE 6

#define LED_GAUCHE 9
#define LED_DROITE 8
#define LED_PECTORAL 10

// Temps en ms mis pour parcourrir 1°, données constructeur
#define VITESSE_ANGULAIRE 2

Servo servoBrasDroit;
Servo servoBrasGauche;
Servo servoTete;

int oldAngleTete       = 90;
int oldAngleBrasDroit  = 90;
int oldAngleBrasGauche = 90;


void blinkLed( int led, int duration)
{
  digitalWrite( led, HIGH);
  delay ( duration);
  digitalWrite( led, LOW);
}

void blinkLeds( int led1, int led2, int duration)
{
  digitalWrite( led1, HIGH);
  digitalWrite( led2, HIGH);
  delay ( duration );
  digitalWrite( led1, LOW);
  digitalWrite( led2, LOW);
}


void setup()
{
  servoBrasDroit.attach( PORT_BRAS_DROIT);  
  servoBrasGauche.attach( PORT_BRAS_GAUCHE);  
  servoTete.attach( PORT_TETE);
  Serial.begin(115200);  

  pinMode( LED_GAUCHE, OUTPUT);
  pinMode( LED_DROITE, OUTPUT);  
  pinMode( LED_PECTORAL, OUTPUT);  

  servoBrasDroit.write( oldAngleBrasDroit);
  servoBrasGauche.write( oldAngleBrasGauche);    
  servoTete.write( oldAngleTete);  

  blinkLeds( LED_GAUCHE, LED_DROITE, 100);
  delay(100);
  blinkLeds( LED_GAUCHE, LED_DROITE, 100);
  delay(100);
  blinkLeds( LED_GAUCHE, LED_DROITE, 100);

  
}

/* La vitesse donnée est un entier compris entre 0 (tres lente) et 255 (vitesse maximale sans palliers) */
void move( Servo * servo, int * oldAngle, int newAngle, int speed )
{
  char chaine[255];
  sprintf(chaine, "moving from %d to %d", *oldAngle, newAngle );
  Serial.println( chaine );
  if ( speed == 255 )
  {
      servo->write( newAngle );
  }
  else
  {
    if ( *oldAngle > newAngle )
    {
      for ( int i = *oldAngle ; i > newAngle ; i--)
      {
        servo->write( i );
        delay( (int) VITESSE_ANGULAIRE * (255 - speed) );
      }
    }
    else if ( *oldAngle < newAngle )
    {
      for ( int i = *oldAngle; i < newAngle ; i ++)
      {
        servo->write( i );
        delay( (int) VITESSE_ANGULAIRE * (255 - speed));
      }
    }
  }
  *oldAngle = newAngle;

}

void loop()
{
  // position repos
  Serial.println("Loop");
  blinkLed( LED_PECTORAL, 500);
  blinkLed( LED_GAUCHE, 100 );
  blinkLed( LED_DROITE, 100 );
  move ( &servoBrasDroit, &oldAngleBrasDroit, 0, 250  );
  move ( &servoTete, &oldAngleTete, 90, 250  );  
  move ( &servoBrasGauche, &oldAngleBrasGauche, 180, 250  );
  blinkLeds( LED_GAUCHE, LED_DROITE, 100);
  blinkLed( LED_PECTORAL, 500 );
  delay(100);
  move ( &servoTete, &oldAngleTete, 0, 250  );  
  move ( &servoTete, &oldAngleTete, 180, 250  );  
  move ( &servoTete, &oldAngleTete, 90, 250  );  
  blinkLeds( LED_GAUCHE, LED_DROITE, 100);
  blinkLed( LED_PECTORAL, 500 );
  delay(100);
  move ( &servoBrasGauche, &oldAngleBrasGauche, 0, 250  );
  move ( &servoBrasDroit, &oldAngleBrasDroit, 180, 250  );
  delay(10000);
 
}


